% 多飞行器协同末制导任务
% 仿真目的：探究飞行时间与末速度之间的对应关系

clc
clear
clear global
global hInt hGui N1 N2 ku kv kr kdelta Td T0 d2r mass g S 
global alphamin alphamax alphaRateLimit bankRateLimit
hInt = 0.01; % 积分步长
hGui = 0.1; % 制导周期
hk = hGui/hInt;
d2r = pi/180;
N1 = 3;
N2 = 3;
ku = 10;
kv = 4;
kr = 1e-6;
kdelta = 8;
g = 9.81;
alphaRateLimit = 5*pi/180;
bankRateLimit = 40*pi/180;
alphamin = -5*pi/180;
alphamax = 25*pi/180;
mass = 1600;
S = 0.5026;

% state0 = [30e3 -30e3  20e3 1600 2*d2r -60*d2r];
% state0 = [25e3  30e3  22e3 1600 2*d2r -130*d2r];
state0 = [-25e3 -30e3  20e3 1600 5*d2r 0*d2r];
%% PNG
num = 6e3;
res = nan(num,17);
res(1, 1:7) = [0 state0];
for i = 2:num 
    res_past = res(i-1,:);
    res_new = PNGguidanceloop(res_past);
    res(i,:) = res_new;
    if res(i, 4) <= 0
        num = i-1;
        break;
    end
end
PNG = res(1:num, :);
T0 = num*hInt;
save('data.mat', 'PNG');
%% ITCG1 时间间隔1s
Td = round(T0 + 8);
hkv = 10;
num = Td/hInt;
res = nan(num,17);
res(1, 1:7) = [0 state0];
res(1, 10) = T0;
for i = 2:num 
    res_past = res(i-1,:);
    if hkv >= hk
        res_new = ITCGguidanceloop(res_past);
        hkv = 0;
    else
        res_new = ITCGloop(res_past);
    end
    res(i,:) = res_new;
    hkv = hkv + 1;
    if res(i, 4) <= 0
        num = i-1;
        break;
    end
end
ITCG1 = res(1:num, :);
save('data.mat', 'ITCG1', '-append');